#ifndef KAI01_HARDWARE_INTERFACE__MECANUM_BASE_HARDWARE_INTERFACE_HPP_
#define KAI01_HARDWARE_INTERFACE__MECANUM_BASE_HARDWARE_INTERFACE_HPP_

#include <string>
#include <vector>
#include "kai01_hardware_interface/visibility_control.h"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp_lifecycle/state.hpp"

#include <iostream>

namespace kai01_hardware_interface
{

  class MecanumBaseHardwareInterface : public hardware_interface::SystemInterface
  {
  public:
    TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
    hardware_interface::CallbackReturn on_init(
        const hardware_interface::HardwareInfo &info) override;

    TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
    hardware_interface::CallbackReturn on_configure(
        const rclcpp_lifecycle::State &previous_state) override;

    TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

    TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

    TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
    hardware_interface::CallbackReturn on_activate(
        const rclcpp_lifecycle::State &previous_state) override;

    TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
    hardware_interface::CallbackReturn on_deactivate(
        const rclcpp_lifecycle::State &previous_state) override;

    TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
    hardware_interface::return_type read(
        const rclcpp::Time &time, const rclcpp::Duration &period) override;

    TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC
    hardware_interface::return_type write(
        const rclcpp::Time &time, const rclcpp::Duration &period) override;

  private:
    // 底盘状态和命令
    std::vector<double> hw_commands_velocity_;
    std::vector<double> hw_states_imudata_;
    std::vector<double> hw_states_velocity_;

    // 串口通信

    std::string device_name_;
    int baudrate_;
    int timeout_;

    // 解析串口数据
    bool parse_serial_data(const std::string &data);
    // 打包命令数据
    std::string pack_command_data();
  };

  

} // namespace kai01_hardware_interface

#endif // KAI01_HARDWARE_INTERFACE__MECANUM_BASE_HARDWARE_INTERFACE_HPP_
